%% This file allows us to solve a PDE (partial differential equation in 2D
% This example shows as we should proceed to solve a Partial Differential
% Equation (PDE) by using Local Max-Entropy (LME) shape functions.
% 
% The problem statement is:
%              | -Lap(f) = -2*y         in Dom,
%              | f = 0                  on the frontier of Dom,
% where Lap means Laplacian, and Dom is the domain, Dom=[0,1]x[0,1]

%addpath(path,'functions');
clear all
close all
clc

%%  Load dependencies

s = cd; path(s, path);
s = cd; s = [s, '/BC']; path(s, path);
s = cd; s = [s, '/EFG']; path(s, path);
s = cd; s = [s, '/EFGChino']; path(s, path);
%s = cd; s = [s, '/EFG/toolbox_local/utilities']; path(s, path);

%% PDE : Heat Equation (CODINA'S problem)
% Nomenclature
%   L2_@: L2 norm error
%   nn_@: nearest neighbors node list
%   n@  : number of samples or nodes
%   x_@ : samples or nodes coordinates
%   h   : nodal spacing
%   p_@ : LME shape functions
%   dp_@: LME shape function derivatives
L       = [1 1];
Nx      = 21;
Ny      = 21;
centre  = [0.5*L(1); 0.5*L(2)];
x_nodes = UniformGrid2D(Nx, Ny, L(1), L(2), centre);
nPts    = size(x_nodes,1);
h       = 1/(Nx-1);

%% Boundary conditions
% bc_homog = 0; %homogeneus (the PDE is modified and appears a source term), 
  bc_homog = 1; %non homogeneus (some nodes have BC imposed)

%% Gauss-Legendre Numerical Integration
% Numerical Integration
optGauss.orderGL = 6;     %cubature order: 2 (3 GPts), 4 (6 GPts), 6 (12 GPts)
bcY1GLOrder=1;
bcY0GLOrder=1;
bcX1GLOrder=1;
bcX0GLOrder=1;
% Sample points where the shape functions are computed
[x_gauss w_gauss]= MakeGLSamples2D(x_nodes, optGauss);

%% EFG paramters
rho = 1.5*h;
%order = 1;
optEFG.rho = rho;
optEFG.poliBaseType = '';
%optEFG.order = order;
% For compatibility------------------------------------------
% Options for the computation of the shape functions
optEFG.dim     = 2;           % spatial dimension
optEFG.verb    = 1;           % information and plots: 0=OFF, 1=ON
optEFG.grad    = 1;           % Computation of the Gradient 0:OFF 1:ON
optEFG.hess    = 0;           % Computation of the Hessian  0:OFF 1:ON
optEFG.TolNR   = 1.e-12;      % Newton-Raphson Tolerance
optEFG.Tol0    = 1.e-6;       % Target Zero Tolerance
% Definition of locality parameters
gamma  = 1.6;
beta   = gamma/h/h;
beta_n = ones(1,nPts)*beta;
optEFG.beta_n  = beta_n;      % locality parameters
range_n        = zeros(nPts,1);
for i=1:nPts
  range_n(i)  = max(2*h,sqrt(-log(optEFG.Tol0)./beta_n(i)));
end
optEFG.range_n = range_n;     % range definition
% end compatibility
range_n = ones(size(x_nodes,1))*4*rho;


% adjacency structure with the nearest neighbors nodes to each sample point
% nodal shape parameter
s_near = SamplesAdjacency_efg(x_nodes, x_gauss, range_n);
% mockSnear = cell(1, size(x_gauss,1));
% for sx = 1 : size(x_gauss,1);
%     mockSnear(sx) = {(1:size(x_nodes,1))};
% end;
% s_near=mockSnear;

% Local-max entropy basis functions computation
optEFG.s_near = s_near;
tic
outEFG = wrapperCHINO(x_nodes,x_gauss,optEFG);
toc

p_samp  = outEFG.p_samp;
dp_samp = outEFG.dp_samp;

%% Modificacion para agregar puntos de gaus en el borde--------------------
% boundary Y=1-------------------------------------------------------------
count = 0;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,2) == 1)
        count = count + 1;
    end;
end;
bcNodesY1 = zeros(count,2);
i=1;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,2) == 1)
        bcNodesY1(i, : ) = x_nodes(ii, : );
        i = i+1;
    end;
end;
bcOPTGauss.orderGL = bcY1GLOrder;
[bc_x_gauss1D bc_w_gaussY1]= MakeGLSamples1D(bcNodesY1(:,1), bcOPTGauss);

bc_x_gaussY1 = zeros(size(bc_x_gauss1D,1), 2);
bc_x_gaussY1(:,1) = bc_x_gauss1D(:);
bc_x_gaussY1(:,2) = 1;
% boundary Y=0-------------------------------------------------------------
count = 0;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,2) == 0)
        count = count + 1;
    end;
end;
bcNodesY0 = zeros(count,2);
i=1;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,2) == 0)
        bcNodesY0(i, : ) = x_nodes(ii, : );
        i = i+1;
    end;
end;
bcOPTGauss.orderGL = bcY0GLOrder;
[bc_x_gauss1D bc_w_gaussY0] = MakeGLSamples1D(bcNodesY0(:,1), bcOPTGauss);

bc_x_gaussY0 = zeros(size(bc_x_gauss1D,1), 2);
bc_x_gaussY0(:,1) = bc_x_gauss1D(:);
bc_x_gaussY0(:,2) = 0;
% boundary X=0-------------------------------------------------------------
count = 0;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,1) == 0)
        count = count + 1;
    end;
end;
bcNodesX0 = zeros(count,2);
i=1;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,1) == 0)
        bcNodesX0(i, : ) = x_nodes(ii, : );
        i = i+1;
    end;
end;
bcOPTGauss.orderGL = bcX0GLOrder;
[bc_x_gauss1D bc_w_gaussX0] = MakeGLSamples1D(bcNodesX0(:,2), bcOPTGauss);

bc_x_gaussX0 = zeros(size(bc_x_gauss1D,1), 2);
bc_x_gaussX0(:,2) = bc_x_gauss1D(:);
bc_x_gaussX0(:,1) = 0;

% boundary X=1-------------------------------------------------------------
count = 0;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,1) == 1)
        count = count + 1;
    end;
end;
bcNodesX1 = zeros(count,2);
i=1;
for ii = 1 : size(x_nodes,1);
    if (x_nodes(ii,1) == 1)
        bcNodesX1(i, : ) = x_nodes(ii, : );
        i = i+1;
    end;
end;
bcOPTGauss.orderGL = bcX1GLOrder;
[bc_x_gauss1D bc_w_gaussX1] = MakeGLSamples1D(bcNodesX1(:,2), bcOPTGauss);

bc_x_gaussX1 = zeros(size(bc_x_gauss1D,1), 2);
bc_x_gaussX1(:,2) = bc_x_gauss1D(:);
bc_x_gaussX1(:,1) = 1;

nX0 = size(bc_x_gaussX0,1);
nX1 = size(bc_x_gaussX1,1);
nY0 = size(bc_x_gaussY0,1);
nY1 = size(bc_x_gaussY1,1);

nnX0 = size(bcNodesX0,1);
nnX1 = size(bcNodesX1,1);
nnY0 = size(bcNodesY0,1);
nnY1 = size(bcNodesY1,1);

% Put all the bcNodes all together
bc_x_gauss = zeros(nX0+nX1+nY0+nY1,2);
bc_w_gauss = zeros(size(bc_x_gauss,1),1);
bcNodes = zeros(nnX0+nnX1+nnY0+nnY1,2);

bcNodes(1:nnX0,:) = bcNodesX0(:,:);
bcNodes(nnX0+1:nnX0+nnX1,:) = bcNodesX1(:,:);
bcNodes(nnX0+nnX1+1:nnX0+nnX1+nnY0,:) = bcNodesY0(:,:);
bcNodes(nnX0+nnX1+nnY0+1:end,:) = bcNodesY1(:,:);

bc_x_gauss(1:nX0,:) = bc_x_gaussX0(:,:);
bc_x_gauss(nX0+1:nX0+nX1,:) = bc_x_gaussX1(:,:);
bc_x_gauss(nX0+nX1+1:nX0+nX1+nY0,:) = bc_x_gaussY0(:,:);
bc_x_gauss(nX0+nX1+nY0+1:end,:) = bc_x_gaussY1(:,:);

bc_w_gauss(1 : nX0) = bc_w_gaussX0(:);
bc_w_gauss(nX0+1 : nX0+nX1) = bc_w_gaussX1(:);
bc_w_gauss(nX0+nX1+1:nX0+nX1+nY0) = bc_w_gaussY0(:);
bc_w_gauss(nX0+nX1+nY0+1:end) = bc_w_gaussY1(:);

% The boundary conditions and penalty values
boundaryConditions = zeros(nX0+nX1+nY0+nY1,1);
if (bc_homog == 1);
    bc = bc_x_gaussY1(:,1) - bc_x_gaussY1(:,1) .* bc_x_gaussY1(:,1);
    boundaryConditions(nX0+nX1+nY0+1:end) = bc(:,1);
end;
% Penalty method parameter
penalty  = 10000;

penaltyN = 10000;
% Caclulates the boundary normal at each sample point
boundaryNormals = zeros(nX0+nX1+nY0+nY1,2);
boundaryNormals(1:nX0,1) = 0;
boundaryNormals(1:nX0,2) = -1;
boundaryNormals(nX0+1:nX0+nX1,1) = 0;
boundaryNormals(nX0+1:nX0+nX1,2) = 1;
boundaryNormals(nX0+nX1+1:nX0+nX1+nY0,1) = -1;
boundaryNormals(nX0+nX1+1:nX0+nX1+nY0,2) = 0;
boundaryNormals(nX0+nX1+nY0+1:end,1) = 1;
boundaryNormals(nX0+nX1+nY0+1:end,2) = 0;

%% Sampling the shape function on the boundary sample nodes----------------
% adjacency structure with the nearest neighbors nodes to each sample point
% nodal shape parameter
bc_s_near = SamplesAdjacency_efg(x_nodes, bc_x_gauss, range_n);
%  bc_mockSnear = cell(1, size(bc_x_gauss,1));
%  for sx = 1 : size(bc_x_gauss,1);
%      bc_mockSnear(sx) = {(1:size(x_nodes,1))};
%  end;
%  bc_s_near=bc_mockSnear;

% Local-max entropy basis functions computation
bc_optEFG = optEFG;
bc_optEFG.s_near = bc_s_near;
bc_outEFG = wrapperCHINO(x_nodes, bc_x_gauss, bc_optEFG);
bc_p_samp  = bc_outEFG.p_samp;
bc_dp_samp = bc_outEFG.dp_samp;

%% The numerical solution is computed
%id_bd: indices for the nodes on the boundary
nPts    = length(x_nodes);
ids     = (1:nPts);
id_bd   = [ids(abs(x_nodes(:,1))<1.e-6) ids(abs(x_nodes(:,2))<1.e-6) ...
           ids(abs(x_nodes(:,1)-L(1))<1.e-6) ids(abs(x_nodes(:,2)-L(2))<1.e-6)];
id_bd   = unique(id_bd);

% Standard solver with FEM-lik boundary condition imposition
%u_nodes = ex23_SolveSystem(bc_homog,nPts,id_bd,x_nodes,s_near,p_samp,dp_samp,x_gauss,w_gauss);
% Standard solver with FEM-lik boundary condition imposition - re-Writed
% u_nodes = codinaSolveMatrix_BC_direct(bc_homog,nPts,id_bd,x_nodes,s_near,p_samp,dp_samp,x_gauss,w_gauss);
% Standard solver with penalty boundary condition imposition
%   u_nodes = codinaSolveMatrix_BC_penalty(bc_x_gauss, boundaryConditions, bc_p_samp, bc_w_gauss, bc_s_near,...
%       penalty, bc_homog,nPts,id_bd,x_nodes,s_near,p_samp,dp_samp,x_gauss,w_gauss );
% Standard solver with Nitsche boundary condition imposition
%    u_nodes = codinaSolveMatrix_BC_nitsche(bc_x_gauss, boundaryConditions, bc_p_samp, bc_w_gauss, bc_s_near, bc_dp_samp, boundaryNormals,...
%        penaltyN, bc_homog,nPts,id_bd,x_nodes,s_near,p_samp,dp_samp,x_gauss,w_gauss );
% Standard solver with boundary condition imposed with Lagrange multipliers
[u_nodesLagrange condK] = codinaSolveMatrix_BC_Lagrange(bc_x_gauss, boundaryConditions, bc_p_samp, bc_s_near,...
    bc_homog,x_nodes,s_near,p_samp,dp_samp,x_gauss,w_gauss );
u_nodes = zeros(size(x_nodes,1),1);
u_nodes(:) = u_nodesLagrange(1:size(u_nodes,1));
 condK

%% Error in L2 norm
% compute value of u in the Gauss points
u_gauss = SamplesSolution(u_nodes,s_near,p_samp);

% analytical value of u in the Gauss points
u_sol   = ex23_AnalyticalSolution(bc_homog,x_gauss);

% L_2 norm of the error
L2_GL   = sqrt(sum((u_gauss-u_sol).^2.*w_gauss));

fprintf('\tNorm L2 of the error : %10.4e\n', L2_GL);



figure(2)
plot3(x_nodes(:,1),x_nodes(:,2),u_nodes,'ro',...
    x_gauss(:,1),x_gauss(:,2),u_gauss, 'bx',...
    x_gauss(:,1),x_gauss(:,2),u_sol, 'k+')
xlabel('X')
ylabel('Y')
zlabel('u(x)')
legend('node points','Gauss points num','Gauss points real')

%% Te ploteo una funcion de forma asi vemos si no estamos haciendo cagadas
nFuncionPlot = size(x_nodes,1);
nf = nFuncionPlot;

l = size(u_nodes,1);
u_plot = zeros (l,1);
%u_plot(nf) = 1;
u_plot(50) = 1;

u_shapeFunction = SamplesSolution(u_plot,s_near,dp_samp);

figure(4)
plot3(x_nodes(:,1),x_nodes(:,2),u_plot,'ro',...
    x_gauss(:,1),x_gauss(:,2),u_shapeFunction, 'bx')
xlabel('X')
ylabel('Y')
zlabel('u(x)')
legend('node points','shape function 50','Gauss points real')
%% Te ploteo una funcion de forma asi vemos si no estamos haciendo cagadas
nFuncionPlot = size(x_nodes,1);
nf = nFuncionPlot;

l = size(u_nodes,1);
u_plot = zeros (l,1);
%u_plot(nf) = 1;
u_plot(50) = 1;

u_shapeFunction = SamplesSolution(u_plot,s_near,p_samp);

figure(5)
plot3(x_nodes(:,1),x_nodes(:,2),u_plot,'ro',...
    x_gauss(:,1),x_gauss(:,2),u_shapeFunction, 'bx')
xlabel('X')
ylabel('Y')
zlabel('u(x)')
legend('node points','shape function 50','Gauss points real')


%% Solution is plot
figure(3);clf
nSY = 50;
nSX = 50;
plotLME2D(x_nodes,u_nodes, nSX, nSY, L(1), L(2), centre, optEFG);
view([20 10])
title(strcat('LME  \gamma = ',num2str(gamma)),'fontsize',18,'fontweight','b')
xlabel('X', 'fontsize',16,'fontweight','b')
ylabel('Y', 'fontsize',16,'fontweight','b')
zlabel('\delta_z','fontsize',16,'fontweight','b')
disp('- - - - - - - -')




% %%  Zero Order Consistency
% ZeroCheck = 0;
% for indexSample = 1:size(x_gauss,1)
%     p = p_samp{indexSample};
%     ZeroCheck = ZeroCheck + sum(p(:));
% end
% ZeroCheck - size(x_gauss,1)
% 
% %%  First Order Consistency
% FirstCheck = zeros(size(x_gauss,1),2);
% for indexSample = 1:size(x_gauss,1)
%     p = p_samp{indexSample};
%     FirstCheck(indexSample,:) = p(:)'*x_nodes;
% end
% diffCheck = FirstCheck - x_gauss;
% max(max(diffCheck))
% 
% %%  Second Order Consistency
% ZeroCheck = zeros(2,1);
% for indexSample = 1 : size(x_gauss,1)
%     dp = dp_samp{indexSample};
%     ZeroCheck(1) = ZeroCheck(1) + sum(dp(:,1));
%     ZeroCheck(2) = ZeroCheck(2) + sum(dp(:,2));
% end
% ZeroCheck(1) - size(x_gauss,1)
% ZeroCheck(2) - size(x_gauss,1)
